{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 移动操作基本运动控制\n",
    "\n",
    "## 硬件准备\n",
    "\n",
    "材料：\n",
    "\n",
    "小车：移动底座 M*1、I模块*4、四麦克朗姆轮 *4 \n",
    "\n",
    "操作臂（5自由度）：I模块*2、T模块*3、G模块*1\n",
    "\n",
    "\n",
    "\n",
    "## 硬件连接\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "## 关节配置\n",
    "\n",
    "将硬件连接的关节ID次序更新到配置文件中\n",
    "\n",
    "\n",
    "\n",
    "配置完后启动设备\n",
    "\n",
    "\n",
    "# 代码说明\n",
    "\n",
    "### 导入库"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#!/usr/bin/python\n",
    "# -*- coding: UTF-8 -*-\n",
    "import sys;\n",
    "import os\n",
    "sys.path.append(\"..\")\n",
    "sys.path.append(\"../../\")\n",
    "from lib.BwRobotLib import BwRobotLib\n",
    "from RobotCar.CarRobot import CarRobot\n",
    "from RobotManipulator.ManipulatorRobot import ManipulatorRobot\n",
    "import keyboard\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 声明库对象"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "#声明库及机器人对象\n",
    "robotlib = BwRobotLib()\n",
    "carRobot = CarRobot(robotlib)\n",
    "manipulator = ManipulatorRobot(robotlib)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 连接设备"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "连接成功\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#连接机器人设备\n",
    "robotlib.connectRobot(\"192.168.1.107\",55001)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 配置小车并初始化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "请选择轮子模式 :\n",
      "2 : 两轮\n",
      "3 : 三轮\n",
      "4 : 四向轮\n",
      "5 : 四麦克朗姆轮\n",
      "6 : 四并橡胶轮\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      " 5\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Total Ids Count=10\n",
      "检测到的模块数： 10\n",
      "Id： 20  Position:8\n",
      "Id： 21  Position:11\n",
      "Id： 22  Position:4\n",
      "Id： 23  Position:6\n",
      "Id： 24  Position:9\n",
      "Id： 30  Position:9\n",
      "Id： 31  Position:9\n",
      "Id： 25  Position:9\n",
      "Id： 32  Position:9\n",
      "Id： 40  Position:9\n",
      "任意键回车继续....... (输入 0 回车重新扫描)\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      " \n"
     ]
    }
   ],
   "source": [
    "#配置轮式模式\n",
    "print(\"请选择轮子模式 :\")\n",
    "print(\"2 : 两轮\")\n",
    "print(\"3 : 三轮\")\n",
    "print(\"4 : 四向轮\")\n",
    "print(\"5 : 四麦克朗姆轮\")\n",
    "print(\"6 : 四并橡胶轮\")\n",
    "mode = robotlib.inputNum()\n",
    "\n",
    "#加载配置\n",
    "robotlib.module_ids=robotlib.scanNConfigCarIds()\n",
    "\n",
    "#配置机器人\n",
    "#配置ID:ids F(4),B(11),l1(8),l2(8),l3(10),l4(11),r1(4),r2(7),r3(6),r4(6) 【TOP:9】\n",
    "carRobot.bindIds(robotlib.module_ids,mode)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 配置操作臂并初始化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "读取ID配置文件:\n",
      "#操作臂\n",
      "\n",
      "UP:24,30,31,25,32,40\n",
      "关节模块24位置：-135\n",
      "关节模块30位置：89\n",
      "关节模块31位置：89\n",
      "关节模块25位置：47\n",
      "关节模块32位置：0\n",
      "关节模块40位置：5\n"
     ]
    }
   ],
   "source": [
    "#加载配置（兼容VS项目启动方式及文件直接启动方式）\n",
    "import os,inspect\n",
    "fileName = inspect.getframeinfo(inspect.currentframe()).filename\n",
    "folderPath     = os.path.dirname(os.path.abspath(fileName))\n",
    "cfile=folderPath+\"/config.txt\"\n",
    "robotlib.loadConfig(cfile)\n",
    "\n",
    "\n",
    "#配置操作臂\n",
    "manipulator.bindIds(robotlib.module_ids)\n",
    "\n",
    "#读取当前坐标\n",
    "manipulator.readAngles()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "### 示例声明\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "#运行\n",
    "def runDemo():\n",
    "  waitSeconds = 2\n",
    "  manipulator.pickUpSample()\n",
    "  carRobot.goForward()\n",
    "  time.sleep(waitSeconds)\n",
    "  carRobot.goBack()\n",
    "  time.sleep(waitSeconds)\n",
    "  carRobot.turnLeft()\n",
    "  time.sleep(1)\n",
    "  carRobot.turnRight()\n",
    "  time.sleep(1)\n",
    "  carRobot.stop()\n",
    "\n",
    "  if(carRobot.Mode==5):\n",
    "      carRobot.runLeft()\n",
    "      time.sleep(waitSeconds)\n",
    "      carRobot.runRight()\n",
    "      time.sleep(waitSeconds)\n",
    "      carRobot.stop()\n",
    "  manipulator.putDownSample();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 主入口"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "输入任意字符 Enter 运行演示示例... \n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "四麦克朗姆轮向前走...\n",
      "四麦克朗姆轮向后走...\n",
      "四麦克朗姆轮左转...\n",
      "四麦克朗姆轮右转...\n",
      "停止\n",
      "四麦克朗姆轮向左走...\n",
      "四麦克朗姆轮向右走...\n",
      "停止\n"
     ]
    }
   ],
   "source": [
    "while(1):\n",
    "  input(\"输入任意字符 Enter 运行演示示例...\")\n",
    "  runDemo()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
